/*
 * Copyright (c) 2006-2020, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2020-09-02     RT-Thread    first version
 */

#include <rtthread.h>
#include <rtdevice.h>
#include "drv_common.h"
#include "drv_spi_ili9488.h"  // spi lcd driver 
#include <lcd_spi_port.h>  // lcd ports
#include <rt_ai_cld_model.h>
#include <rt_ai.h>
#include <backend_cubeai.h>
#include "sht3x.h"
#include "yumi.h"


#define LED_PIN   GET_PIN(I, 8)
#define MOTOR_PIN GET_PIN(E, 6)


rt_uint8_t Motor_flag = 0;

struct rt_event ov2640_event;
rt_uint8_t *input_gray;
rt_uint8_t *input_gray_160;
float *input_buf;
rt_uint8_t ai_flag = 0;

extern void DCMI_Start();
extern int rt_gc0328c_init();
static inline void _itof(float *dst,rt_uint8_t *src, uint32_t size, float div);
void rgb2gray(unsigned char *src,unsigned char *dst, int width,int height);
void bilinera_interpolation(rt_uint8_t* in_array, short height, short width, 
                            rt_uint8_t* out_array, short out_height, short out_width);


//char *corn_label[] = {"health", "big spot", "small spot", "rust"};
char *corn_label[] = {"health", "disease", "disease", "disease"};
rt_err_t result = RT_EOK;
int ai_run_complete_flag = 0;
void ai_camera();

void ai_run_complete(void *arg){
   *(int*)arg = 1;
}
int main(void)
{
    rt_pin_mode(LED_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(MOTOR_PIN, PIN_MODE_OUTPUT);

//    rt_pin_mode(M_PIN, PIN_MODE_OUTPUT);
//    rt_pin_mode(SCL_PIN, PIN_MODE_OUTPUT);
//    rt_pin_mode(SDA_PIN, PIN_MODE_OUTPUT);
//
//    rt_pin_write(M_PIN, PIN_LOW);
//    rt_pin_write(SCL_PIN, PIN_LOW);
//    rt_pin_write(SDA_PIN, PIN_LOW);

    /*sht30 init */
    static sht3x_device_t dev = RT_NULL;
    rt_uint8_t sht_addr = SHT3X_ADDR_PD ;
    dev = sht3x_init("i2c4", sht_addr);

    input_gray = rt_malloc(320*240);
    input_gray_160 = rt_malloc(160*160);
    input_buf = rt_malloc(160*160*sizeof(float));
    rt_ai_buffer_t *work_buffer = rt_malloc(RT_AI_CLD_WORK_BUFFER_BYTES+RT_AI_CLD_IN_TOTAL_SIZE_BYTES+RT_AI_CLD_OUT_TOTAL_SIZE_BYTES);
    
    /* init spi data notify event */
    rt_event_init(&ov2640_event, "ov2640", RT_IPC_FLAG_FIFO);
    struct drv_lcd_device *lcd;
    lcd = (struct drv_lcd_device *)rt_device_find("lcd");
    struct rt_device_rect_info rect_info = {0, 0, LCD_WIDTH, 240};
    lcd->parent.control(&lcd->parent, RTGRAPHIC_CTRL_RECT_UPDATE, &rect_info);

    // find ai model handle
    rt_ai_t model = NULL;
    model = rt_ai_find(RT_AI_CLD_MODEL_NAME);
        if(model == RT_AI_NULL){
            return -1;
        }

     //init the model handle
     result = rt_ai_init(model, work_buffer);
     if(result != 0){
         return -1;
         }

    ai_camera();
    lcd_show_image(0,240,320,240,YUMI);
    while(1){
        rt_pin_write(LED_PIN, PIN_LOW);


        sht3x_read_singleshot(dev);
        //rt_kprintf("sht3x humidity   : %d.%d \n", (int)dev->humidity, (int)(dev->humidity * 10) % 10);
        //rt_kprintf("sht3x temperature: %d.%d \n", (int)dev->temperature, (int)(dev->temperature * 10) % 10);

        rt_event_recv(&ov2640_event,
                        1,
                        RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR,
                        RT_WAITING_FOREVER,
                        RT_NULL);
        rt_pin_write(LED_PIN, PIN_HIGH);
        lcd->parent.control(&lcd->parent, RTGRAPHIC_CTRL_RECT_UPDATE, &rect_info);
//        if(ai_flag > 0){
        //            rt_kprintf("ai start");
            rgb2gray(lcd->lcd_info.framebuffer ,input_gray, 320,240);
            bilinera_interpolation(input_gray, 240, 320, input_gray_160, 160, 160);
            rt_memcpy(model->input[0], input_gray_160, RT_AI_CLD_IN_1_SIZE_BYTES);
            result = rt_ai_run(model,ai_run_complete, &ai_run_complete_flag);

            //process the inference data
            if(ai_run_complete_flag){
                //get inference data
                uint8_t *out = (uint8_t *)rt_ai_output(model, 0);
                //get argmax
                int max_index = 0;
                for(int i = 1 ; i < 4 ; i++){
                     if(out[i] > out[max_index]){
                         max_index = i;
                     }
                }
              if(max_index == 0)
                  {
                      rt_pin_write(MOTOR_PIN, PIN_LOW);
                      Motor_flag = 0;
                  }
              else
                  if(Motor_flag > 3)
                      rt_pin_write(MOTOR_PIN, PIN_HIGH);
                  else
                      Motor_flag++;

              lcd_show_string(10, 240, 16, "health status: %s%s", corn_label[max_index],"      ");
              lcd_show_string(10, 256, 16, "humidity     : %d.%d RH", (int)dev->humidity, (int)(dev->humidity * 10) % 10 );
              lcd_show_string(10, 272, 16, "temperature  : %d.%d C", (int)dev->temperature, (int)(dev->temperature * 10) % 10);

        DCMI_Start();
        }  
    }
    
    return RT_EOK;
}

#include "stm32h7xx.h"
static int vtor_config(void)
{
   /* Vector Table Relocation in Internal QSPI_FLASH */
   SCB->VTOR = QSPI_BASE;
   return 0;
}
INIT_BOARD_EXPORT(vtor_config);

void ai_camera()
{
    rt_gc0328c_init();
    ai_flag = 1;
    DCMI_Start();
}
// MSH_CMD_EXPORT(ai_camera, Start the AI camera to recognize person);

static inline void _itof(float *dst,rt_uint8_t *src, uint32_t size, float div){
    if (div == 0){
        return ;
    }else{
        int i = 0;
        for(i = 0; i < size; i++){
            dst[i] = (float)src[i] / div;
        }
    }
}

// img covnert to gray: Gray = 0.2989*R + 0.5870*G + 0.1140*B
// better: 4898*R + 9618*G + 1868*B >> 14
// int8: 76*R + 150*G + 30*B >> 8
void rgb2gray(unsigned char *src, unsigned char *dst, int width, int height)
{
    int r, g, b;
    for (int i=0; i<width*height; ++i)
    {
        r = *src++; // load red
        g = *src++; // load green
        b = *src++; // load blue
        // build weighted average:
        *dst++ = (r * 76 + g * 150 + b * 30) >> 8;
    }
}

int is_in_array(short x, short y, short height, short width)
{
    if (x >= 0 && x < width && y >= 0 && y < height)
        return 1;
    else
        return 0;
}

void bilinera_interpolation(rt_uint8_t* in_array, short height, short width, 
                            rt_uint8_t* out_array, short out_height, short out_width)
{
    double h_times = (double)out_height / (double)height,
           w_times = (double)out_width / (double)width;
    short  x1, y1, x2, y2, f11, f12, f21, f22;
    double x, y;

    for (int i = 0; i < out_height; i++){
        for (int j = 0; j < out_width; j++){
            x = j / w_times;
            y = i / h_times;
          
            x1 = (short)(x - 1);
            x2 = (short)(x + 1);
            y1 = (short)(y + 1);
            y2 = (short)(y - 1);
            f11 = is_in_array(x1, y1, height, width) ? in_array[y1*width+x1] : 0;
            f12 = is_in_array(x1, y2, height, width) ? in_array[y2*width+x1] : 0;
            f21 = is_in_array(x2, y1, height, width) ? in_array[y1*width+x2] : 0;
            f22 = is_in_array(x2, y2, height, width) ? in_array[y2*width+x2] : 0;
            out_array[i*out_width+j] = (rt_uint8_t)(((f11 * (x2 - x) * (y2 - y)) +
                                       (f21 * (x - x1) * (y2 - y)) +
                                       (f12 * (x2 - x) * (y - y1)) +
                                       (f22 * (x - x1) * (y - y1))) / ((x2 - x1) * (y2 - y1)));
        }
    }
}
